// gcode preprocessor for the RepRap Wally by Nicholas Seward. Before using,
// adjust all sections marked CALLIBRATE.

// throughout this file, we refer to reference, machine, and actual coordinates.
// actual: in mm, (0,0,0) is at extended arms and lowered bed.
// reference: in mm, (0,0,0) is at the left shoulder and lowered bed.
// machine: in microsteps, these are the values that will be fed to the firmware.

package main

import (
	"bufio"
	"fmt"
	"io"
	"log"
	"math"
	"os"
	"strconv"
	"strings"
)

// callibrated constants
const (
	microstepping    = 16.0   // microstepping level of the stepper drivers
	armStepsPerMm    = 8.0    // set in firmware
	armLength        = 150.0  // length of a forearm
	armDist          = 250.0  // distance from shoulder to shoulder
	straightForearms = 1053.0 // d for which G1 Xd Yd makes the arms colinear

	// Z = 0 at the bottom, Z decreases during printing
	//zDirection = -1.0
	//zSquare    = 76.0  // Z height (mm) where bed arms are at 90 degrees
	//zTouch     = 134.6 // Z height (mm) where the nozzle barely touched the bed

	// Z = 0 at the top, Z increases during printing
	zDirection = 1.0
	zSquare    = 68.0
	zTouch     = 0.0

	segmentSize = 1 // maximum size of line segment
)

// Here are some magic numbers that I don't understand.
// my y gets skewed downward as x increases.
// the rate of skew increases as y increases.
const (
	yTop     = 211.25     // not real top, just for this slop measurement
	yTopSkew = 2.4 / 38.5 // at y = yTop, y drops this much per x

	yBottom     = 98.75
	yBottomSkew = 1.9 / 38.5 // y = yBottom, y drops this much per x

	// skew increases at this rate as y increases
	ySkewSlope = (yTopSkew - yBottomSkew) / (yTop - yBottom)

	xyScale = 1.00

	yzSkew = 1.0 / 10.0 // y skews up this much per mm of z
)

// calculated constants
var (
	stepsPerRotation = (360.0 / 1.8) * microstepping
	mmPerRotation    = stepsPerRotation / armStepsPerMm
	initialAngle     = math.Acos(armDist / (4 * armLength))
	mechAdvantage    = (straightForearms/(mmPerRotation/2.0)*math.Pi + math.Asin(1-armDist/2.0/armLength) + math.Asin(armDist/4.0/armLength)) / (math.Pi - math.Acos(1-armDist/2.0/armLength))
)

var (
	lastFeedrate  float64
	input         *bufio.Reader
	output        *bufio.Writer
	prev, current point
)

type point struct{ x, y, z, e, f float64 }

func (p point) debug() string {
	return fmt.Sprintf("X%.3f Y%.3f Z%.3f E%.5f F%.3f", p.x, p.y, p.z, p.e, p.f)
}

// Fix y to keep up with the z arc, and fix some skewing
func fixY(p point) point {
	// first fix skew (probably happens in arms , z shouldn't factor in)
	skew := yBottomSkew + ySkewSlope*(p.y-yBottom)
	p.y += skew * p.x

	// now fix for Z
	deltaZ := p.z - zSquare
	d := math.Sqrt(armLength*armLength - deltaZ*deltaZ)
	adjust := armLength - d // adjust by 0 when z=zSquare (d=armLength)
	p.y += adjust
	//p.y += yzSkew * p.z

	return p
}

func (p point) String() string {
	var s string
	s += fmt.Sprintf("X%.3f", p.x)
	s += fmt.Sprintf(" Y%.3f", p.y)
	s += fmt.Sprintf(" Z%.3f", p.z)
	s += fmt.Sprintf(" E%.5f", p.e)
	if p.f != lastFeedrate {
		s += fmt.Sprintf(" F%.3f", p.f)
		lastFeedrate = p.f
	}
	return s
}

// dist returns the distance between two points.
func dist(a, b point) float64 {
	if a.x == b.x && a.y == b.y && a.z == b.z {
		return 0.0
	}
	return math.Sqrt(math.Pow(a.x-b.x, 2) + math.Pow(a.y-b.y, 2) + math.Pow(a.z-b.z, 2))
}

// linearInterp chooses a point between a and b.
func linearInterp(a, b point, segnum, segmax int) point {
	var out point
	i, n := float64(segnum), float64(segmax)
	out.x = (i*b.x + (n-i)*a.x) / n
	out.y = (i*b.y + (n-i)*a.y) / n
	out.z = (i*b.z + (n-i)*a.z) / n
	out.e = (i*b.e + (n-i)*a.e) / n
	out.f = b.f
	return out
}

// segmentize splits a line from a to b into segments.
func segmentize(a, b point) []point {
	l := dist(a, b)
	if l <= segmentSize {
		return []point{b}
	}
	n := int(math.Ceil(l / segmentSize))
	out := make([]point, n)
	for i := 0; i < n; i++ {
		out[i] = linearInterp(a, b, i, n)
	}
	return out
}

// transform converts a point from cartesian coordinates to Wally coordinates.
func transform(p point) point {

	// last minute quick hacky fix
	p.x *= xyScale
	p.y *= xyScale

	l := armLength

	// get (x,y) in reference coords
	x := armDist/2.0 + p.x
	// y distance from backboard to fully extended arms
	yDist := math.Sqrt((2.0*armLength)*(2.0*armLength) - (0.5*armDist)*(0.5*armDist))
	y := p.y - yDist

	left_leg := math.Sqrt(x*x + y*y)
	right_leg := math.Sqrt((armDist-x)*(armDist-x) + y*y)
	left_elbow_tmp := (left_leg*left_leg - 2*l*l) / (-2 * l * l)
	if left_elbow_tmp < -1.0 || left_elbow_tmp > 1.0 {
		fmt.Fprintln(os.Stderr, "left:", left_elbow_tmp)
		if left_elbow_tmp < -1.0 {
			left_elbow_tmp = -1.0
		} else {
			left_elbow_tmp = 1.0
		}
	}
	right_elbow_tmp := (right_leg*right_leg - 2*l*l) / (-2 * l * l)
	if right_elbow_tmp < -1.0 || right_elbow_tmp > 1.0 {
		fmt.Fprintln(os.Stderr, "right:", right_elbow_tmp)
		if right_elbow_tmp < -1.0 {
			right_elbow_tmp = -1.0
		} else {
			right_elbow_tmp = 1.0
		}
	}
	left_elbow := math.Acos(left_elbow_tmp)
	right_elbow := math.Acos(right_elbow_tmp)
	left_small_angle := (math.Pi - left_elbow) / 2
	right_small_angle := (math.Pi - right_elbow) / 2
	left_virtual := math.Atan(-y / x)
	right_virtual := math.Atan(-y / (armDist - x))
	left_drive := left_small_angle + left_virtual - initialAngle
	right_drive := right_small_angle + right_virtual - initialAngle
	left_stepper := -left_drive + (math.Pi-left_elbow)*mechAdvantage
	right_stepper := -right_drive + (math.Pi-right_elbow)*mechAdvantage
	xMachine := left_stepper * mmPerRotation / 2 / math.Pi
	yMachine := right_stepper * mmPerRotation / 2 / math.Pi
	if math.IsNaN(xMachine) || math.IsNaN(yMachine) {
		panic("error: got NaN while calculating arm movements")
	}
	zMachine := p.z // TODO: this incorrectly assumes actual and machine z are the same.
	return point{xMachine, yMachine, zMachine, p.e, p.f}
	return p
}

func main() {
	input = bufio.NewReader(os.Stdin)
	output = bufio.NewWriter(os.Stdout)
	current, prev = point{}, point{}
	var move, yMove bool
	for {
		move, yMove = false, false
		line, err := input.ReadString('\n')
		if err == io.EOF {
			break
		} else if err != nil {
			log.Fatal(err)
		}
		line = strings.ToUpper(strings.Split(line, ";")[0])
		command := strings.Fields(line)
		if len(command) < 1 {
			continue
		}
		if command[0] == "G28" {
			// don't respect G28 - home all axes
			continue
		}
		// TODO: hack
		if command[0] == "G92" && command[1] == "E0" {
			prev.e, current.e = 0, 0
			fmt.Fprintln(output, strings.Join(command, " "))
		} else if command[0] == "G1" {
			for _, part := range command[1:] {
				axis := part[0]
				v, err := strconv.ParseFloat(part[1:], 64)
				if err != nil {
					log.Fatal(err)
				}
				switch axis {
				case 'X':
					move = true
					current.x = v
				case 'Y':
					move, yMove = true, true
					current.y = v
				case 'Z':
					move = true
					current.z = zTouch + zDirection*v
				case 'E':
					current.e = v
				case 'F':
					current.f = v
				default:
					log.Fatal("invalid axis in G1 command")
				}
			}
			if yMove {
				current = fixY(current)
			}
			if move && current.e != prev.e {
				// print move; segmentize
				var points []point
				points = segmentize(prev, current)
				//fmt.Fprintf(output, "; segments for (%v) to (%v):\n", prev.debug(), current.debug())
				for _, p := range points {
					fmt.Fprintln(output, "G1", transform(p))
				}
			} else {
				if move == true {
					fmt.Fprintln(output, "G1", transform(current))
					//fmt.Fprintln(output, "G1", current)
				} else {
					fmt.Fprintln(output, strings.Join(command, " "))
				}
			}
			prev = current
		} else {
			fmt.Fprintln(output, strings.Join(command, " "))
		}
	}
	//output.WriteString("M18\n") // disable all motors
	output.WriteString("G1 X0 Y0\n") // get ready for next print
	output.Flush()
}
